#include "gyro_bot_motor.h"
#include <QDebug>
#include <QElapsedTimer>

gyro_bot_motor::gyro_bot_motor(char* port)
{
    this->port=port;
    this->MotorCom=new Posix_QextSerialPort(port,QextSerialBase::Polling);
}

void gyro_bot_motor::Connect()
{
    MotorCom->open(QIODevice::ReadWrite);
    MotorCom->setBaudRate(BAUD115200);
    MotorCom->setDataBits(DATA_8);
    MotorCom->setParity(PAR_NONE);
    MotorCom->setStopBits(STOP_1);
    MotorCom->setFlowControl(FLOW_OFF);
    MotorCom->setTimeout(200);
    MotorCom->write("\n");
}

void gyro_bot_motor::GetVol()
{
    MotorCom->write("/r/n1,GCT/r/n");
    MotorCom->write("/r/n1,GCT/r/n");
    MotorCom->write("/r/n1,GCT/r/n");
}

void gyro_bot_motor::SetSPD_SVS(const char* mot,int spd)
{
    QString temp="\r\n1,SVS,";
    temp+=mot;
    temp+=",";
    temp+=QString::number(spd);
    temp+="\r\n";
    MotorCom->write(temp.toAscii());
    //qDebug()<<temp;
}

void gyro_bot_motor::SetSPD_SVR(int m1spd,int m2spd)
{
    QString temp="\r\n1,SVR,1,";
    temp+=QString::number(m1spd);
    temp+="\r\n";
    MotorCom->write(temp.toAscii());
    temp="\r\n1,SVR,2,";
    temp+=QString::number(m2spd);
    temp+="\r\n";
    MotorCom->write(temp.toAscii());
}

void gyro_bot_motor::SetSPD_AVR()
{
    MotorCom->write("\n\r1,AVR,1,1\n\r");
}

void gyro_bot_motor::test()
{
    MotorCom->write("\n1,SVS,1,100\r\n");
}
